#include "LDPC.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <stdint.h>
#include "arm_math.h"
#include "float.h"

const float32_t G_matrix[info_length][code_length]={
	{1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 1, 1, 1, 0, 0, 0, 0, 1, 1, 0, 1, 0, 0, 0, 0, 0}, 
	{0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 1, 1, 1, 0, 1, 0, 0, 1, 0, 0, 1, 0, 1, 0, 0, 0, 0}, 
	{0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 1, 0, 1, 1, 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0}, 
	{0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 1, 0, 1, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0}, 
	{0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 1}, 
	{0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 1, 0}, 
	{0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0}, 
	{0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1}, 
	{0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 1, 1, 0, 0, 1, 0, 0, 1, 1, 1, 0, 0, 0, 0, 1, 1, 0}, 
	{0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 0, 1, 1, 0, 0, 0, 1, 1, 0, 1, 0, 0, 1, 0, 0, 1}, 
	{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 1, 1, 1, 0, 0, 1, 0, 1, 0, 1, 1, 0, 1, 1, 0, 0, 0, 0, 1}, 
	{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 1, 1, 0, 0, 0, 1, 0, 1, 1, 1, 1, 0, 0, 1, 0, 0, 1, 0}, 
	{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 1, 0, 0, 1, 0, 1, 1, 0, 0, 0}, 
	{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 1, 1, 0, 1, 0, 1, 1, 0, 1, 0, 0, 1, 0, 0}, 
	{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 1, 0, 0, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 0, 1, 0}, 
	{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 0, 0, 1, 0, 0, 1, 0, 1}
};

float val;
uint8_t start;
uint8_t col_weight;
uint8_t num_edges;
float total_sign;
float min1;
float min2;
float32_t abs_val;
uint8_t edge_idx;
float sign;
float other_sign;
float min_val;
float sum_uml;
uint8_t weight;
float qn;
uint8_t sum;
uint8_t valid;

arm_matrix_instance_f32 mat_digital, mat_LDPC_digital, mat_G_matrix;

// 稀疏矩阵结构
struct SparseMatrix{
    uint8_t col_weights[code_length];      // 每列非零元素数
    uint8_t row_weights[parity_length];      // 每行非零元素数
    uint8_t col_start[code_length];        // 每列在消息数组的起始位置
    uint8_t total_edges;       // 非零元素总数
    uint8_t row_edge_index[parity_length][6];  // 每行非零元素的消息索引
    uint8_t row_col_index[parity_length][6];   // 每行非零元素的列索引
};

struct SparseMatrix H_sparse = {
	{2,2,2,2,2,2,2,2,4,4,4,4,4,4,4,4,2,2,2,2,2,2,2,2,1,1,1,1,1,1,1,1,6,6,6,6,6,6,6,6},
	{3,3,3,3,3,3,3,3,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6},
	{0,2,4,6,8,10,12,14,16,20,24,28,32,36,40,44,48,50,52,54,56,58,60,62,64,65,66,67,68,69,70,71,72,78,84,90,96,102,108,114},
	120,
	{
		{64,72,90},
		{65,78,84},
		{66,85,96},
		{67,91,102},
		{68,97,114},
		{69,103,108},
		{70,79,109},
		{71,73,115},
		{0,14,16,28,36,74},
		{2,12,20,24,32,80},
		{1,4,25,37,44,86},
		{3,6,29,33,40,92},
		{7,8,21,34,41,98},
		{5,10,17,38,45,104},
		{11,13,18,30,42,110},
		{9,15,22,26,46,116},
		{19,48,54,93,105,117},
		{23,50,52,87,99,111},
		{27,53,58,75,100,118},
		{31,55,56,81,106,112},
		{35,57,60,76,94,119},
		{39,59,62,82,88,113},
		{43,51,61,83,95,107},
		{47,49,63,77,89,101}
	},
	{
		{24,32,35},
		{25,33,34},
		{26,34,36},
		{27,35,37},
		{28,36,39},
		{29,37,38},
		{30,33,38},
		{31,32,39},
		{0,7,8,11,13,32},
		{1,6,9,10,12,33},
		{0,2,10,13,15,34},
		{1,3,11,12,14,35},
		{3,4,9,12,14,36},
		{2,5,8,13,15,37},
		{5,6,8,11,14,38},
		{4,7,9,10,15,39},
		{8,16,19,35,37,39},
		{9,17,18,34,36,38},
		{10,18,21,32,36,39},
		{11,19,20,33,37,38},
		{12,20,22,32,35,39},
		{13,21,23,33,34,38},
		{14,17,22,33,35,37},
		{15,16,23,32,34,36}
	}
};


void LDPC_encode(float32_t *digital, float32_t *LDPC_digital)
{
	arm_mat_init_f32(&mat_digital, 1, info_length, (float32_t *)digital);
	arm_mat_init_f32(&mat_LDPC_digital, 1, code_length, (float32_t *)LDPC_digital);
	arm_mat_init_f32(&mat_G_matrix, info_length, code_length, (float32_t *)G_matrix);
	
	arm_mat_mult_f32(&mat_digital, &mat_G_matrix, &mat_LDPC_digital);
	
	for(uint16_t i=0;i<code_length;i++)
	{
		LDPC_digital[i] = (uint16_t)LDPC_digital[i]%2;
	}
	
}

// LDPC��С�����뺯��
void LDPC_decode(const float* LDPC_digital, uint8_t* digital)
{
    static float *vml = NULL;
    static float *uml = NULL;
    static float *qn0_1 = NULL;

    // ����Ƶ�����ڴ����
    if (!vml) {
        vml = (float *)malloc(H_sparse.total_edges * sizeof(float));
        uml = (float *)malloc(H_sparse.total_edges * sizeof(float));
        qn0_1 = (float *)malloc(code_length * sizeof(float));
    }

    // ��ʼ��vml�������ظ����㣬�����ڴ����
    for (uint16_t j = 0; j < code_length; j++) 
		{
        val = LDPC_digital[j];
        start = H_sparse.col_start[j];
        col_weight = H_sparse.col_weights[j];
        for(uint16_t k = 0; k < col_weight; k++) 
				{
            vml[start + k] = val;
        }
    }

    for (uint16_t iter = 0; iter < max_iter; iter++) {
        // У��ڵ����
        for (uint16_t i = 0; i < parity_length; i++) {
            num_edges = H_sparse.row_weights[i];
            if (num_edges == 0) continue;

            // ������ų˻�����Сֵ
						total_sign = 1.0f;
            min1 = FLT_MAX;
						min2 = FLT_MAX;
            uint8_t *row_edge_idx = H_sparse.row_edge_index[i];

            // ʹ�� arm_abs_f32 ����� fabsf
            for (uint16_t k = 0; k < num_edges; k++) {
                val = vml[row_edge_idx[k]];
                if (val < 0.0f) total_sign *= -1.0f;
                
                // ʹ�� arm_abs_f32 ��� fabsf��Ӳ������
                abs_val = 0.0f;
                arm_abs_f32(&val, &abs_val, 1);

                if (abs_val <= min1) {
                    min2 = min1;
                    min1 = abs_val;
                } else if (abs_val < min2) {
                    min2 = abs_val;
                }
            }

            // ����У��ڵ���Ϣ
            for (uint16_t k = 0; k < num_edges; k++) {
                edge_idx = row_edge_idx[k];
                val = vml[edge_idx];
                sign = (val >= 0.0f) ? 1.0f : -1.0f;
                other_sign = total_sign * sign;
                min_val = (fabsf(val) == min1) ? min2 : min1;
                uml[edge_idx] = other_sign * min_val * norm_factor;
            }
        }

        // �����ڵ���� + ����������
        for (uint16_t j = 0; j < code_length; j++) {
            sum_uml = 0.0f;
            start = H_sparse.col_start[j];
            weight = H_sparse.col_weights[j];

            // ʹ�� arm_add_f32 ����������
            arm_add_f32(&uml[start], &uml[start + weight], &sum_uml, weight);

            // �������
            qn = LDPC_digital[j] + sum_uml;
            qn0_1[j] = qn;

            // ���� vml
            for (uint16_t k = 0; k < weight; k++) {
                vml[start + k] = qn - uml[start + k];
            }
        }

        // Ӳ�о�
        for (uint16_t j = 0; j < code_length; j++) {
            digital[j] = (qn0_1[j] >= 0.0f) ? 1 : 0;
        }

        // У��
        valid = 1;
        for (uint16_t i = 0; i < parity_length; i++) {
            sum = 0;
            uint8_t *row_col_idx = H_sparse.row_col_index[i];

            // ʹ�� arm_xor ���㰴λ���
            for (uint8_t k = 0; k < H_sparse.row_weights[i]; k++) {
                sum ^= digital[row_col_idx[k]];
            }

            // У�鲻ͨ������ǰ�˳�
            if (sum != 0) {
                valid = 0;
                break;
            }
        }

        // У��ͨ������ǰ�˳�
        if (valid) return;
    }

    // �ͷ��ڴ棨���ÿ�ε���ʱ�������ͷţ�
    // free(vml);
    // free(uml);
    // free(qn0_1);
}
